#! /usr/bin/python3


from jetcam.csi_camera import CSICamera
import cv2
import rospy
from sensor_msgs.msg import Image
import sys
sys.path.remove('/opt/ros/melodic/lib/python2.7/dist-packages')
import cv_bridge
import queue
import threading
from cv_bridge import CvBridge, CvBridgeError



class Camera:
	def __init__(self, camera):
		self.cap = camera
		self.q = queue.Queue()
		t = threading.Thread(target = self._reader)
		t.deamon = True
		t.start()

	def _reader(self):
		while True:
			frame = self.cap.read()
			if not self.q.empty():
				try:
					self.q.get_nowait()
				except queue.Empty:
					pass
			self.q.put(frame)
	def read(self):
		return self.q.get()

#camera1 = CSICamera(capture_device=1, width=960, height=640, capture_fps=30)


def pubImage():
	camera0 = Camera(CSICamera(capture_device=0, 
		width=960, 
		height=640, 
		capture_width=1280, 
		capture_height=720,
		capture_fps=20))
	camera1 = Camera(CSICamera(capture_device=1,
		width=960,
		height=640,
		capture_width=1280,
		capture_height=720,
		capture_fps=20))
	print("camera init success")
	rospy.init_node("pubImage", anonymous=True)
	pub0 = rospy.Publisher('ShowImage_0', Image, queue_size = 1)
	pub1 = rospy.Publisher('ShowImage_1', Image, queue_size = 1)
	rate = rospy.Rate(20)
	bridge = CvBridge()
	while not rospy.is_shutdown():
		image0 = camera0.read()
		image1 = camera1.read()
#		cv2.imshow("image0", image0)
#		cv2.imshow("image1", image1)
#		cv2.waitKey(1)
		print("success")
		pub0.publish(bridge.cv2_to_imgmsg(image0, "bgr8"))
		pub1.publish(bridge.cv2_to_imgmsg(image1, "bgr8"))		
		rate.sleep()

if __name__ == "__main__":
	try:
		pubImage()
	except rospy.ROSInterruptException:
		print("failed")
		pass
#while 1:
#    image0 = camera0.read()
#    image1 = camera1.read()
#    cv2.imshow("CSI Camera0", image0)
    
#    cv2.imshow("CSI Camera1", image1)
#    kk = cv2.waitKey(1)
#    if kk == ord('q'):  
#        break
